/*
Arm Tracker and Glove Demo by Noah Zerkin (noazark AT gmail.com) 2009
Released under GNU GPLv3.0 http://www.opensource.org/licenses/gpl-3.0.html
*/

class Bullet{

  float[] trajectory = new float[3];
  float[] currentCoords = new float[3];

  Bullet(float[] launchPosition, float[] launchedVector){
    currentCoords = launchPosition;
    trajectory[0] = launchedVector[0];
    trajectory[1] = launchedVector[1];
    trajectory[2] = launchedVector[2];
  }

  void update(){
    currentCoords[0] += trajectory[0];
    currentCoords[1] += trajectory[1];
    currentCoords[2] += trajectory[2];
  }

  boolean isFlying(){
    boolean flying = false;
    if(abs(currentCoords[0]) < 10000 && abs(currentCoords[1]) < 10000 && abs(currentCoords[2]) < 10000){
      flying = true;
    }
    return flying;
  }

  float[] getPosition(){
    return currentCoords;
  }

  float getX(){
    return currentCoords[0];
  }

  float getY(){
    return currentCoords[1];
  }

  float getZ(){
    return currentCoords[2];
  }

  float[] getVector(){
    return trajectory;
  }

  void setPosition(float[] positionInput){
    currentCoords = positionInput;
  }

  void display(){
    pushMatrix();
    translate(currentCoords[0], currentCoords[1], currentCoords[2]);
    box(3,3,3);
    popMatrix();
  }

}

class Gun{

  float[] globalPosition = new float[3];
  float[] gripPosition = new float[3];
  float[] muzzlePosition = new float[3];
  float[] chamberPosition = new float[3];
  float[] launchVector = new float[3];
  float[] rotations = new float[5];
  boolean inHand = false;

  Gun(float posX, float posY, float posZ){
    globalPosition[0] = posX;
    globalPosition[1] = posY;
    globalPosition[2] = posZ;
  }

  void display(){
    grab();
    if(inHand){
      held();
    }
    pushMatrix();
    if(!inHand){
      translate(globalPosition[0], globalPosition[1], globalPosition[2]);
    }
    //    rotateY(rotations[1]);
    //    rotateY(rotations[3]);
    //    rotateX(rotations[0]-HALF_PI);
    //    rotateZ(rotations[2]);
    //     rotateY(rotations[4]);
    stroke(#ADFF2F);
    translate(3,3,0);
    rotateZ(0.25);
    pushMatrix();
    rotateZ(0.35);
    gripPosition[0] = modelX(0,0,0); 
    gripPosition[1] = modelY(0,0,0); 
    gripPosition[2] = modelZ(0,0,0);
    box(10, 15, 3); 
    popMatrix();
    pushMatrix();
    translate(0,-8,0);
    chamberPosition[0] = modelX(0,0,0); 
    chamberPosition[1] = modelY(0,0,0); 
    chamberPosition[2] = modelZ(0,0,0);
    muzzlePosition[0] = modelX(30,0,0); 
    muzzlePosition[1] = modelY(30,0,0); 
    muzzlePosition[2] = modelZ(30,0,0);
    pushMatrix();
    translate(28,-4,0);
    box(3,3,1);
    translate(-28,0,1);
    box(3,2,1);
    translate(0,0,-2);
    box(3,2,1);
    popMatrix();
    translate(15, 0, 0);
    box(30, 6, 4);
    translate(15,0,0);
    box(2,2,2);
    translate(-20, 5, 0);
    box(2,4,1);
    popMatrix();
    for(byte i = 0; i < 3; i++){
      launchVector[i] = (muzzlePosition[i] - chamberPosition[i]);
    }
    if(inHand && myPose > 2000){
      fire();
    }
    popMatrix();
  }

  void grab(){
    if(dist(palmX, palmY, palmZ, gripPosition[0], gripPosition[1], gripPosition[2]) < 6 && (myPose == 1444 || myPose == 3444 || myPose == 2444 || myPose == 4444)){
      inHand = true;
    }
    if(myPose == 1111){
      inHand = false;
    }
  }

  boolean holding(){
    return inHand;
  }

  void held(){
    globalPosition[0] = palmX;
    globalPosition[1] = palmY;
    globalPosition[2] = palmZ;
    rotations[1] = 0;
    rotations[1] = yRot[0];
    rotations[2] = glovePitch[0];
    if(gloveZTilt[0] >= HALF_PI){
      rotations[0] = gloveRoll[0];
    }
    else{
      rotations[0] = -gloveRoll[0]+PI;
    }
    rotations[3] = radians(wrist[0][1]);
    rotations[4] = radians(wrist[0][0]);
  }

  float[] getGrip(){
    return gripPosition; 
  }

  float[] getChamber(){
    return chamberPosition; 
  }

  float[] getMuzzle(){
    return muzzlePosition; 
  }

  void fire(){
    bullets.add(new Bullet(muzzlePosition, launchVector));
  }

}

